void writePoints()
{
    int mem;

	// number of waypoints, add 1 for home
	wp_total = (sizeof(mission)/20) +1;
	
	// save the alitude above home option
	if(HOLD_CURRENT_ALT == 1){
		eeprom_busy_wait();
		eeprom_write_byte((uint8_t *)	EE_CONFIG, 0);
	}else{
		eeprom_busy_wait();
		eeprom_write_byte((uint8_t *)	EE_CONFIG, 8);
	}
	
	eeprom_busy_wait();
	eeprom_write_dword((uint32_t *)	EE_ALT_HOLD_HOME, ALT_TO_HOLD * 100);
	
	// save # of points
	eeprom_busy_wait();
	eeprom_write_byte((uint8_t *)	EE_WP_TOTAL, wp_total);

	// save index
	eeprom_busy_wait();
	eeprom_write_byte((uint8_t *)	EE_WP_INDEX, 0);

	// save WP_Radius
	eeprom_busy_wait();
	eeprom_write_byte((uint8_t *)	EE_WP_RADIUS, WP_RADIUS);
	
	// save Loiter Radius
	eeprom_busy_wait();
	eeprom_write_byte((uint8_t *)	EE_LOITER_RADIUS, 	LOITER_RADIUS);

	// set home;
	struct Location loc = {0,0,0,0,0};
	set_waypoint_with_index(loc,0);
	
    for (byte i = 0; i < wp_total-1; i++){
		loc.id = (byte)mission[i][0];
		loc.p1 = (byte)mission[i][1];		

		if(loc.id < 0x20){
			// these are waypoints
    		loc.lat = (long)(mission[i][3] * t7);
    		loc.lng = (long)(mission[i][4] * t7);
	    	loc.alt = (long)mission[i][2] * 100;
		}else{
			// these are raw values
    		loc.lat = (long)(mission[i][3]);
    		loc.lng = (long)(mission[i][4]);
	    	loc.alt = (long)mission[i][2];
	}
		
		set_waypoint_with_index(loc,(i+1));
		/*
    	Serial.print((i+1),DEC);
    	Serial.print(": ");
    	Serial.print(loc.id,DEC);
    	Serial.print(", ");
    	Serial.print(loc.p1,DEC);
    	Serial.print(", ");
    	Serial.print(loc.alt,DEC);
    	Serial.print(", ");
    	Serial.print(loc.lat,DEC);
    	Serial.print(", ");
    	Serial.println(loc.lng,DEC);
		*/
	}	
}


void readPoints(){
	eeprom_busy_wait();
	wp_total = eeprom_read_byte((uint8_t *)	EE_WP_TOTAL);

	struct Location temp = get_wp_with_index(0);

	Serial.print("home #0 ");
	Serial.print("\t id: ");
	Serial.print(temp.id,DEC);
	Serial.print("\tp1: ");
	Serial.print(temp.p1,DEC);
	Serial.print("\talt: ");
	Serial.print(temp.alt,DEC);
	Serial.print(" cm\tlat: ");
	Serial.print(temp.lat,DEC);
	Serial.print("\tlong: ");
	Serial.println(temp.lng,DEC);

    for (byte i = 0; i < wp_total-1; i++){
		temp = get_wp_with_index(i+1);
		Serial.print("command # ");
		Serial.print((i+1),DEC);
		Serial.print("\t id: ");
		Serial.print(temp.id,DEC);
		Serial.print("\tp1: ");
		Serial.print(temp.p1,DEC);
		Serial.print("\talt: ");
		Serial.print(temp.alt,DEC);
		Serial.print("\tlat: ");
		Serial.print(temp.lat,DEC);
		Serial.print("\tlong: ");
		Serial.println(temp.lng,DEC);
	}
}

struct Location get_wp_with_index(int i)
{
	struct Location temp;
	uint32_t mem = WP_START_BYTE + (i * WP_SIZE);

	eeprom_busy_wait();
	temp.id = eeprom_read_byte((uint8_t*)mem);

	mem++;
	eeprom_busy_wait();
	temp.p1 = eeprom_read_byte((uint8_t*)mem);

	mem++;	
	eeprom_busy_wait();
	temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
	
	mem += 4;
	eeprom_busy_wait();
	temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
	
	mem += 4;
	eeprom_busy_wait();
	temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
	
	return temp;
}

void set_waypoint_with_index(Location wp, uint16_t i)
{
	uint32_t mem = WP_START_BYTE + (i * WP_SIZE);

	eeprom_busy_wait();
	eeprom_write_byte((uint8_t *) mem, wp.id);

	mem++;
	eeprom_busy_wait();
	eeprom_write_byte((uint8_t *) mem, wp.p1);

	mem++;
	eeprom_busy_wait();
	eeprom_write_dword((uint32_t *) mem, wp.alt);

	mem += 4;
	eeprom_busy_wait();
	eeprom_write_dword((uint32_t *) mem, wp.lat);

	mem += 4;
	eeprom_busy_wait();
	eeprom_write_dword((uint32_t *) mem, wp.lng);
}